/*
 * rosserial PubSub Example
 */

#include <ros.h>
#include <sensor_msgs/JointState.h>
#include <math.h>

ros::NodeHandle  nh;

void messageCb_joint_state( const sensor_msgs::JointState& state){
  if(state.velocity[1]>0)
  {
    digitalWrite(7,LOW);
  }
  else
  {
    digitalWrite(7,HIGH);
  }

  if(state.velocity[0]>0)
  {
    digitalWrite(4,HIGH);
  }
  else
  {
    digitalWrite(4,LOW);
  }

  analogWrite(6,(int)(fabs(state.velocity[1])*255/2500));
  analogWrite(5,(int)(fabs(state.velocity[0])*255/2500)); 
}

ros::Subscriber<sensor_msgs::JointState> sub_joint_state("columba_joint_state", &messageCb_joint_state);

void setup()
{
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  nh.initNode();
  nh.subscribe(sub_joint_state);
}

void loop()
{
  nh.spinOnce();
  delay(10);
}
